#include "MatixInvert.h"

/* Invert a row-major (C-style) 4x4 matrix. */
void invertMatrix(float *b, float *a)
{
	//float* a=m;
	//float* b=out;
	float c[16]={1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1};
	float da[16], db[16];

	for (int t=0;t<16;t++)
	{
		b[t]=c[t];
	}
		
	int i1;

	// Loop over cols of a from left to right, eliminating above and below diag
	for (int j=0; j<4; j++)
	{   
		// Find largest pivot in column j among rows j..3
		i1 = j;		    // Row with largest pivot candidate
		for (int i=j+1; i<4; i++)
			if (fabs(a[i*4+j]) > fabs(a[i1*4+j]))
				i1 = i;

		// Swap rows i1 and j in a and b to put pivot on diagonal
		float tmp[4];

		for (int g=0;g<4;g++)
		{
			tmp[g]=a[i1*4+g];
			a[i1*4+g]=a[j*4+g];
			a[j*4+g]=tmp[g];

			tmp[g]=b[i1*4+g];
			b[i1*4+g]=b[j*4+g];
			b[j*4+g]=tmp[g];
		}

		// Scale row j to have a unit diagonal
		if (a[j*4+j]==0.0)
		{
			printf("singular, return\n");
			return;
		}

		for (int g=0;g<16;g++)
		{
			da[g]=a[g];
		}

		for (int g=0;g<4;g++)
		{
			b[j*4+g]/=da[j*4+j];
			a[j*4+g]/=da[j*4+j];
		}

		for (int g=0;g<16;g++)
		{
			da[g]=a[g];
			db[g]=b[g];
		}

		// Eliminate off-diagonal elems in col j of a, doing identical ops to b
		for (int i=0; i<4; i++)
		{
			if (i!=j)
			{
				for (int g=0;g<4;g++)
				{
					b[i*4+g]-=da[i*4+j]*db[j*4+g];
					a[i*4+g]-=da[i*4+j]*da[j*4+g];
				}

				for (int g=0;g<16;g++)
				{
					da[g]=a[g];
					db[g]=b[g];
				}
				
			}
		}

		

		
	}
	return;
}

void transposeMatrix( float *out, const float *m )
{
	for (int i=0;i<4;i++)
	{
		for (int j=0;j<4;j++)
		{
			out[i*4+j]=m[j*4+i];
		}
	}
}
